
#include <BGearRestraint>
#include "member_BGearRestraint.h"
#include "member_BRestraint.h"
#include "member_BNode.h"

using namespace BWE;

#define member					(*(member_BGearRestraint*)_ptr)
#define member_allocate()		_ptr = new member_BGearRestraint(this)
#define member_release()		delete (member_BGearRestraint*)_ptr

BGearRestraint::BGearRestraint()
{
	member_allocate();
}
BGearRestraint::BGearRestraint(const BString& name) : BRestraint(name)
{
	member_allocate();
}
BGearRestraint::BGearRestraint(BBody* bodyA, const BVector& pivotA) : BRestraint(bodyA, pivotA)
{
	member_allocate();
}
BGearRestraint::BGearRestraint(BBody* bodyA, BBody* bodyB) : BRestraint(bodyA, bodyB)
{
	member_allocate();
}
BGearRestraint::BGearRestraint(BBody* bodyA, const BVector& pivotA, BBody* bodyB, const BVector& pivotB) : BRestraint(bodyA, pivotA, bodyB, pivotB)
{
	member_allocate();
}
BGearRestraint::~BGearRestraint()
{

}

void BGearRestraint::copy(const BRestraint* other)
{
	BRestraint::copy(other);
}
BRestraint* BGearRestraint::clone() const
{
	BGearRestraint* restraint = new BGearRestraint();
	restraint->copy(this);
	return restraint;
}

bool BGearRestraint::setAxisA(BReal ax, BReal ay, BReal az)
{
	return setAxisA(BVector(ax, ay, az));
}
bool BGearRestraint::setAxisA(const BVector& axisA)
{
	if (member.axisA != axisA)
	{
		member.axisA = axisA;
		return true;
	}
	return false;
}
const BVector& BGearRestraint::axisA() const
{
	return member.axisA;
}

bool BGearRestraint::setAxisB(BReal ax, BReal ay, BReal az)
{
	return setAxisB(BVector(ax, ay, az));
}
bool BGearRestraint::setAxisB(const BVector& axisB)
{
	if (member.axisB != axisB)
	{
		member.axisB = axisB;
		return true;
	}
	return false;
}
const BVector& BGearRestraint::axisB() const
{
	return member.axisB;
}

void BGearRestraint::setRatio(BReal ratio)
{
	if (member.ratio != ratio)
	{
		member.ratio = ratio;
	}
}
BReal BGearRestraint::ratio() const
{
	return member.ratio;
}

int BGearRestraint::solverSize()
{
	return 1;
}
void BGearRestraint::soverItems(BSolverItem* solverItems, BReal stepTime)
{
	const BBody* bodyA = restraint_member(this)->bodyA;
	const BBody* bodyB = restraint_member(this)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;

	BVector axisA = matrixA.mult3(member.axisA);
	BVector axisB = matrixB.mult3(member.axisB);
	solverItems[0].torqueAxisA = axisA;
	solverItems[0].torqueAxisB = axisB * member.ratio;
}
